
#include <dispatch_system/one_way_road.h>


namespace moying
{
namespace navigation
{

    
OneWayRoad::OneWayRoad(){
    static unsigned int next_one_road_id = 1;
    max_vehicle_count_ = 1;
    id_ = next_one_road_id++;
    cur_orientation_        = kUndefine;
    temp_fix_orientation_   = kUndefine;
}

bool OneWayRoad::set_available_orientation(std::vector<Orientation> orients){
    if(orients.size() == 1){
        available_orientation_  = orients; 
        temp_fix_orientation_   = cur_orientation_ = orients[0];
        return true;
    }else if(orients.size() != 2){
        WARNING("orientation number in one way road is not valid ");
        return false;
    }

    if(abs(orients[0]-orients[1]) != 4){
        WARNING("orientation must be opposite");
        return false;
    }

    available_orientation_ = orients; 
    return true;
}

bool OneWayRoad::set_cur_orientation(Orientation orient){ 

    for(auto ori : available_orientation_){
        if(ori == orient){
            cur_orientation_ = orient;
            return true;
        }
    }
    return false; 
}


void OneWayRoad::handleVehiclePoseUpdate(VehiclePtr vehicle,MapCell cell){

    if( isCellInRoad(cell) ){
        addVehicle(vehicle);
        //  printf("leave add one way road\n");
    }else{

        removeVehicle(vehicle);
        // printf("leave remove one way road\n");
    }
}


bool OneWayRoad::addVehicle(VehiclePtr vehicle){

    // printf("enter add one way road\n");
    if( vehicle == nullptr)
        return false;

    if(available_orientation_.empty()){
        printf("one way road hasn't available orientation\n");
        return false;
    }

    std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);

    for(auto iter = vehicles_in_road_.begin();iter != vehicles_in_road_.end();iter++){
        if( iter->vehicle->getID() == vehicle->getID() ){
            auto vi = *iter;
            vi.exit_time = 0;
            vehicles_in_road_.erase(iter);
            vehicles_in_road_.insert(vi);
            return true;
        }
    }

    auto ret = vehicles_in_road_.insert( VehicleInfo(vehicle) );
    if(ret.second){
        vehicle->set_one_way_road_id(id_);
        //如果单向道方向未确定,根据当前机器人朝向更接近哪个可通行方向,设置单向道为哪个方向
        int robot_theta = vehicle->getCloseTheta(vehicle->getPose().th);
        if(cur_orientation_ == kUndefine){  
            int min_dis = 100;
            int index=0;
            for(int i=0;i<available_orientation_.size();i++){
                auto orient = available_orientation_[i];
                int dis = abs(robot_theta-orient);
                if(dis > 4)
                    dis = 8-dis;
                if(dis < min_dis){
                    min_dis = dis;
                    index = i;
                }
            }
            cur_orientation_ = available_orientation_[index];
            printf("set one way road orientation:%d road_id:%d\n",cur_orientation_,id_);
        }else if(robot_theta != cur_orientation_){
            printf("robot's theta is not equal with road_theta,robot_theta:%d, road_orientation:%d\n",robot_theta,cur_orientation_);
        }
        printf("add robot to one way road, robot_id:%d road_id:%d road_orientation:%d robot_size:%d\n",
                vehicle->getID(),id_,cur_orientation_,vehicles_in_road_.size());
    }
    return ret.second;
}

bool OneWayRoad::removeVehicle(VehiclePtr vehicle){
//  printf("enter remove one way road\n");
    if(vehicle == nullptr)
        return false;
    
    sch::ConfigInfo& cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();
    const auto& tr_cfg = cfg_info.traffic_manager_cfg;

    std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
    for(auto iter = vehicles_in_road_.begin();iter != vehicles_in_road_.end();){
        if(iter->vehicle->getID() == vehicle->getID()
        ){

            //机器人离开窄道指定秒数后才从窄道删除
            if(iter->exit_time == 0){
                // iter->exit_time = time(0);
                auto vi = *iter;
                vi.exit_time = time(0);
                vehicles_in_road_.erase(iter);
                vehicles_in_road_.insert(vi);
                return false;
            }else if(time(0) - iter->exit_time < tr_cfg.occupy_seconds_after_leave_one_way_road){
                return false;
            }

            vehicles_in_road_.erase(*iter++);
            vehicle->set_one_way_road_id(0);

            int size = vehicles_in_road_.size();
            if(size == 0){
                if(available_orientation_.size() > 1){
                    cur_orientation_        = kUndefine;
                    temp_fix_orientation_   = kUndefine;
                }
                refuse_enter_times_ = 0;
            }else if(size == 1 && available_orientation_.size() > 1){    //剩下一个机器人时,如果通道方向和机器人方向不一致,设为机器人朝向最接近的方向
                VehiclePtr v = vehicles_in_road_.begin()->vehicle;
                int theta = v->getCloseTheta(v->getPose().th);
                int min_dis = 100;
                int index=0;
                for(int i=0;i<available_orientation_.size();i++){
                    auto orient = available_orientation_[i];
                    int dis = abs(theta-orient);
                    if(dis > 4)
                        dis = 8-dis;
                    if(dis < min_dis){
                        min_dis = dis;
                        index = i;
                    }
                }
                Orientation old = cur_orientation_;
                cur_orientation_ = available_orientation_[index];
                if(old != cur_orientation_){
                    printf("change one way road orientation:%d when left one robot road_id:%d\n",cur_orientation_,id_);
                }
            }
            printf("remove robot from one way road,robot_id:%d road_id:%d road_orientation:%d robot_size:%d\n",
                vehicle->getID(),id_,cur_orientation_,vehicles_in_road_.size());
            return true;
        }else{
            iter++;
        }
    }

    return false;
}

void OneWayRoad::addCell(MapCell cell){

    cells_in_road_.emplace_back(cell);
    if(cells_in_road_.size() == 1){
        minx_ = maxx_ = cell.x;
        miny_ = maxy_ = cell.y;
    }else{
        minx_ = cell.x < minx_ ? cell.x : minx_;
        maxx_ = cell.x > maxx_ ? cell.x : maxx_;
        miny_ = cell.y < miny_ ? cell.y : miny_;
        maxy_ = cell.y > maxy_ ? cell.y : maxy_;
    }
}

void OneWayRoad::mergeOneWayRoad(OneWayRoadPtr other_road,bool clear_other_road){

    if(other_road->id() == id_)
        return;
    for(auto cell : other_road->cells_in_road_){
        cells_in_road_.emplace_back(cell);
    }
    if(clear_other_road){
        other_road->cells_in_road_.clear();
        other_road->minx_ = other_road->maxx_ = other_road->miny_ = other_road->maxy_ = 0;
    }
}

int OneWayRoad::roadLength(){
    
    int x_dis = abs(minx_-maxx_);
    int y_dis = abs(miny_-maxy_);
    int len = round(sqrt(x_dis*x_dis+y_dis*y_dis));

    for(auto ori :available_orientation_){
        if(ori == 0){
            return x_dis;
        }else if(ori == 1 || ori == 3){
            return len;
        }else if(ori == 2){
            return y_dis;
        }
    }
    return len;
}

bool OneWayRoad::isVehicleInRoad(VehiclePtr vehicle){

    std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
    for(auto v : vehicles_in_road_){
        if(v.vehicle->getID() == vehicle->getID()){
            return true;
        }
    }
    return false;

}

bool OneWayRoad::isVehicleInRoad(VehiclePtr vehicle,VehicleInfo& vi){

    std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
    for(auto v : vehicles_in_road_){
        if(v.vehicle->getID() == vehicle->getID()){
            vi = v;
            return true;
        }
    }
    return false;

}

bool OneWayRoad::isCellInRoad(MapCell cell){
    for(auto c : cells_in_road_){
        if(c.x == cell.x && c.y == cell.y)
            return true;
    }
    return false;
}

bool OneWayRoad::isAvailabeOrientation(Orientation orient){
    for(auto ori : available_orientation_){
        if(ori == orient)
            return true;
    }
    return false;
}

void OneWayRoad::increaseRefuseEnterTimes(){

    if(++refuse_enter_times_ >= 2){
        // if(cur_orientation_ != kUndefine){
        //     temp_fix_orientation_ = cur_orientation_;
        //     printf("set temp fix orientation, road_id:%d orient:%d\n",id_,temp_fix_orientation_);
        // }
    }
}

bool OneWayRoad::canEnterRoad(Orientation orientation,VehiclePtr vehicle){

    if(isVehicleInRoad(vehicle))
        return true;
    if(vehicles_in_road_.size() >= max_vehicle_count_)
        return false;
    if( cur_orientation_ != kUndefine && cur_orientation_ != orientation) 
        return false;

    return true;
}

void OneWayRoad::printfInfo(){

    printf("one way road id:%d length:%d cur_direction:%d\n",id_,roadLength(),cur_orientation_);
    for(auto orient : available_orientation_){
        printf(" available orientation:%d\n",orient);
    }
    for(auto vi : vehicles_in_road_){
        printf("robot in road:%d\n",vi.vehicle->getID());
    }
    for(auto cell : cells_in_road_){
        printf(" cell_x:%d cell_y:%d\n",cell.x,cell.y);
    }
}

void OneWayRoad::printRobots(){
    for(auto vi : vehicles_in_road_){
        printf("robot in road, robot_id:%d road_id:%d\n",vi.vehicle->getID(),vi.vehicle->getID());
    }
}


int OneWayRoad::getCloseTheta(int th){

    int min_dis = 100;
    int index=0;
    for(int i=0;i<available_orientation_.size();i++){
        auto orient = available_orientation_[i];
        int dis = abs(th-orient);
        if(dis > 4)
            dis = 8-dis;
        if(dis < min_dis){
            min_dis = dis;
            index = i;
        }
    }
    th = available_orientation_[index];
    return th;
}

void OneWayRoad::getAllVehicles(std::vector<unsigned int>& vehicle_ids){

    std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
    for(auto& v : vehicles_in_road_){
        vehicle_ids.push_back(v.vehicle->getID());
    }
    
}

}
}
